#include <iostream>
using namespace std;
#include <stdio.h>
#include <cmath>

#include <mex.h>
#include "mexFileIO.h"

#include <vector>
#include <algorithm>
#include <queue>
#include <limits>
#include <map>
using namespace std;
#include <mexFileIO.h>
#include <szMexUtility.h>
#include <szMexUtilityTemplate.h>
#include <szMyNeighborOp.h>
#include <szMiscOperations.h>
#include <szConvexHull2D.h>
#include <ContourEQW.h>
#include <szParticleF.h>
#include <Triangulation.h>
#include <FragmentInfo.h>
#include <TriangulationHelper.h>
#include <DistanceMapUtility.h>
#include <DotGroupUtility.h>
#include <Graph.h>
#include <GraphFactory.h>
#include <Kruskal.h>
#include <cassert>
#include <IntersectionConvexPolygons.h>

struct struct_triangle
{
	CParticleF a, b, c;
	struct_triangle()
	{
	}
	struct_triangle(CParticleF& a, CParticleF& b, CParticleF& c)
	{
		this->a = a;
		this->b = b;
		this->c = c;
	}
};

vector<struct_triangle>
getTriangles(vector<CParticleF>& points, vector<int>& I, const int* dims)
{
	vector<struct_triangle> triangles;
	for (int i = 0; i < dims[0]; ++i)
	{
		int idx[3];
		for (int j = 0; j < 3; ++j)
		{
			idx[j] = GetData2(I, i, j, dims[0], dims[1], 0) - 1;
		}
		triangles.push_back(struct_triangle(points[idx[0]], points[idx[1]], points[idx[2]]));
	}
	return triangles;
}

bool
overlappingTriangles(struct_triangle& s, struct_triangle& t)
{
	vector<CParticleF> v(3);
	v[0] = s.a;
	v[1] = s.b;
	v[2] = s.c;
	for (int i = 0; i < 3; ++i)
	{
		if (inside(t.a, v))
		{
			return true;
		}
	}
	return false;
}

float
areaOverlap(struct_triangle& s, struct_triangle& t)
{
	if (overlappingTriangles(s, t) == false) return 0.0f;

	vector<CParticleF> sv(3);
	vector<CParticleF> tv(3);
	sv[0] = s.a; sv[1] = s.b; sv[2] = s.c;
	tv[0] = t.a; tv[1] = t.b; tv[2] = t.c;
	vector<CParticleF> cv = _IntersectConvexPolygon::intersectConvexHulls(sv, tv);
	return polygonArea(cv);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{   
	printf("%s: This build was compiled at %s %s\n", "OverlapArea", __DATE__, __TIME__);
	if (nrhs < 1 || nlhs < 0)
	{
		mexErrMsgTxt("Usage: areas = OverlapArea(P, I, Q, J)");
		return;
	}
	//Points
	vector<CParticleF> P;
	{
		vector<float> P0;
		mxClassID classIdP;
		int ndimP;
		const int* dimsP;
		LoadData(P0, prhs[0], classIdP, ndimP, &dimsP);
		P = vector2particle(P0, dimsP);
	}
	//triangle indices
	vector<int> I;
	const int* dimsI;
	{
		mxClassID classIdP;
		int ndimP;
		LoadData(I, prhs[1], classIdP, ndimP, &dimsI);
	}
	//Points
	vector<CParticleF> Q;
	{
		vector<float> P0;
		mxClassID classIdP;
		int ndimP;
		const int* dimsP;
		LoadData(P0, prhs[2], classIdP, ndimP, &dimsP);
		Q = vector2particle(P0, dimsP);
	}
	//triangle indices
	vector<int> J;
	const int* dimsJ;
	{
		mxClassID classIdP;
		int ndimP;
		LoadData(J, prhs[3], classIdP, ndimP, &dimsJ);
	}
	assert(dimsI[1] >= 3 && dimsJ[1] >= 3);
	vector<struct_triangle> polygon1 = getTriangles(P, I, dimsI);
	vector<struct_triangle> polygon2 = getTriangles(Q, J, dimsJ);
	float area1 = 0, area2 = 0, area3 = 0;
	for (int i = 0; i <polygon1.size(); ++i)
	{
		area1 += areaTriangle(polygon1[i].a, polygon1[i].b, polygon1[i].c);
	}
	for (int i = 0; i <polygon2.size(); ++i)
	{
		area2 += areaTriangle(polygon2[i].a, polygon2[i].b, polygon2[i].c);
	}
	for (int i = 0; i < polygon1.size(); ++i)
	{
		for (int j = 0; j < polygon2.size(); ++j)
		{
			area3 += areaOverlap(polygon1[i], polygon2[j]);
		}
	}

	if(nlhs >= 1)
	{
		const int dims[] = {3, 1};
		vector<float> F(dims[0]*dims[1]);
		F[0] = area1;
		F[1] = area2;
		F[2] = area3;
		plhs[0] = StoreData(F, mxSINGLE_CLASS, 2, dims);
	}
	mexUnlock();
}

